#include "globalVars.hpp"
#include "GlobalDefs.hpp"
#include <random>

char GlobalVars::outputBuffer[SERIAL_OT_BUFFER_SIZE + 1];
char GlobalVars::inputBuffer[SERIAL_IN_BUFFER_SIZE + 1];


//const static int trigRecNum = 256;
uint32_t GlobalVars::fan0Trigs[trigRecNum], GlobalVars::fan1Trigs[trigRecNum], GlobalVars::fan2Trigs[trigRecNum];
int GlobalVars::fan0TrigsIdx = -1, GlobalVars::fan1TrigsIdx = -1, GlobalVars::fan2TrigsIdx = -1;

String GlobalVars::identifier;

uint8_t GlobalVars::tempState = 0;
GlobalVars::PointTemperature GlobalVars::plateTemp;
GlobalVars::PointTemperature GlobalVars::sampleLeftTemp;
GlobalVars::PointTemperature GlobalVars::sampleRightTemp;
GlobalVars::PointTemperature GlobalVars::pumpTemp;
GlobalVars::PointTemperature GlobalVars::containerTemp;
GlobalVars::PointTemperature GlobalVars::surroundingTemp;
GlobalVars::PointTemperature GlobalVars::coreTemp;

uint8_t GlobalVars::heatUpDrivePercent = 0;
uint8_t GlobalVars::coolDownDrivePercent = 0;
uint8_t GlobalVars::fanDrivePercent = 0;
uint8_t GlobalVars::drivesDisable = 0;

TemperatureControlPlan GlobalVars::ctrlPlan;
int GlobalVars::flagSimuRate = 0;
int GlobalVars::flagAutoReportInterval = -1;
int GlobalVars::flagAutoReportCurve = -1;
char GlobalVars::flagAutoPrintSingle = 0;
int GlobalVars::flagVerboseMode = 0;
int GlobalVars::flagSampleObMode = 0;

float GlobalVars::temptarget = 0;
float GlobalVars::mean_Reading = 0;

float GlobalVars::PIDoutputP = 0;
float GlobalVars::PIDoutputI = 0;
float GlobalVars::PIDoutputD = 0;
unsigned long GlobalVars::timeMs = 0;

float GlobalVars::speed = 0;
float GlobalVars::changetarget = 0;
int GlobalVars::fanSpeedRPSs[3];

TemperatureController* GlobalVars::heatUpCurve;
TemperatureController* GlobalVars::heatHoldCurve;
TemperatureController* GlobalVars::coolDownCurve;
TemperatureController* GlobalVars::coolHoldCurve;

ThermalModel GlobalVars::thermalModel;
MainTempRecord GlobalVars::mainTempRecord;

int GlobalVars::OnBoot::controlMode = 0;
float GlobalVars::OnBoot::tempTarget = 0;
int GlobalVars::OnBoot::verbose = 0;


unsigned long GlobalVars::MiscFlags::heartBeatExpire = 0;
int GlobalVars::MiscFlags::heartBeatInterval = 0;

void GlobalVars::init()
{
	plateTemp.warningThreshold = 125;
	sampleLeftTemp.warningThreshold = 125;
	sampleRightTemp.warningThreshold = 125;
	pumpTemp.warningThreshold = 100;
	containerTemp.warningThreshold = 115;
	surroundingTemp.warningThreshold = 60;
	coreTemp.warningThreshold = 100;

#ifdef CONTROL_BANGBANG
	heatUpCurve = new BangBangController();
	heatHoldCurve = new BangBangController();
	coolDownCurve = new BangBangController();
	coolHoldCurve = new BangBangController();
#endif
#ifdef CONTROL_MODE_PID
	heatUpCurve = new PIDController();
	heatHoldCurve = new PIDController();
	coolDownCurve = new PIDController();
	coolHoldCurve = new PIDController();
#endif
	coolDownCurve->direction = -1;
	//coolHoldCurve->direction = -1;
}

void GlobalVars::restartCurves() {
	auto time = clock();
	heatUpCurve->restart(time);
	heatHoldCurve->restart(time);
	coolDownCurve->restart(time);
	coolHoldCurve->restart(time);
	mainTempRecord.restart();
}

GlobalVars::PointTemperature::PointTemperature()
{
	readingVal = ERROR_TEMP_NOT_DEFINED;
	readingTimeMs = 0;
	targetVal = ERROR_TEMP_NOT_DEFINED;
	readingTimeMs = 0;
}

void GlobalVars::PointTemperature::setSimuThermalModel(float affectedBySourceRateIn, float convergeToSurroundingRateIn, bool sourceRateAbsoluteIn) {
	affectedBySourceRate = affectedBySourceRateIn;
	convergeToSurroundingRate = convergeToSurroundingRateIn;
	affectedBySourceAbsolute = sourceRateAbsoluteIn;
}

void GlobalVars::PointTemperature::setSimuRandom(float initial, float target)
{
	readingVal = initial;
	targetVal = target;
	readingTimeMs = millis();
	if (GlobalVars::flagSimuRate > 0) {
		readingTimeMs *= GlobalVars::flagSimuRate;
	}
}

//void GlobalVars::PointTemperature::simuRead(float sourceTemp, float surroundingTemp, bool flagVerbose)
void GlobalVars::PointTemperature::simuRead(float sourceTempOrChangeRate, float surroundingTemp, float relativeConvergeToSurroundingRate, bool flagVerbose)
{
	delay(10);
	auto newReadTime = millis();
	if (GlobalVars::flagSimuRate > 0) {
		newReadTime *= GlobalVars::flagSimuRate;
	}

	// default: transfer of energy based on difference
	float fromSource = (sourceTempOrChangeRate - readingVal) * affectedBySourceRate;
	if (affectedBySourceAbsolute) {
		// for the plate directedly heated
		fromSource = sourceTempOrChangeRate * affectedBySourceRate;
	}
	float fromSurrounding = (surroundingTemp - readingVal) * convergeToSurroundingRate * (1 + relativeConvergeToSurroundingRate);
	float error = 0;
	if (affectedBySourceRate == 0) {
		// fluctuate 0.3C
		error = (rand() % 61 - 30) / 100.0;
	}

	float changePerSec = fromSource + fromSurrounding + error;
	float elapsedTime = (newReadTime - readingTimeMs) / 1000.0f;
	readingVal += changePerSec * elapsedTime;

	if (flagVerbose) {
		Serial.print("Simu read ");
		Serial.print(",src=");
		Serial.print((int)(sourceTempOrChangeRate));
		Serial.print(",srd=");
		Serial.print((int)(surroundingTemp * 10));
		Serial.print(",frc=");
		Serial.print((int)(fromSource * 100));
		Serial.print(",frd=");
		Serial.print((int)(fromSurrounding * 100));
		Serial.print(",rt=");
		Serial.print((int)(changePerSec * 1000));
		Serial.print(",tm=");
		Serial.print((int)(elapsedTime * 1000));
		Serial.println();
	}

	//const float passiveRampRate = 0.5;
	//float rampRate = passiveRampRate;
	//if (GlobalVars::ctrlPlan.tempFeedbackMode > 0)
	//{
	//	rampRate = GlobalVars::ctrlPlan.tempCtrlRampRate * relativeRampRate;
	//}
	//float elapsedTime = (millis() - readingTimeMs) / 1000.0f * GlobalVars::flagSimuRate;

	//float randChange = (rampRate + (rand() % 30) / 300.0) * elapsedTime;
	//if (readingVal < targetVal)
	//{
	//	readingVal += randChange;
	//}
	//else
	//{
	//	readingVal -= randChange;
	//}
	readingTimeMs = newReadTime;
}

void GlobalVars::PointTemperature::extrasimuRead(float* sourceTempA, float* sourceTempB)
{
	float fromSource1 = (*sourceTempA - readingVal) * 0.1;
	float fromSource2 = (*sourceTempB - readingVal) * 0.1;
	float error = 0;
	float changePerSec1 = fromSource1 + error;
	float changePerSec2 = fromSource2 + error;
	readingVal += changePerSec1 + changePerSec2;
	*sourceTempA -= changePerSec1;
	*sourceTempB -= changePerSec2;
}

bool GlobalVars::PointTemperature::reachedWarningThresh()
{
	if (readingVal < TEMP_VALID_MIN)
	{
		return false;
	}

	return readingVal >= warningThreshold;
}

bool GlobalVars::PointTemperature::nearWarningThresh() {
	return nearWarningThresh(readingVal);
}

bool GlobalVars::PointTemperature::nearWarningThresh(float reading) {
	if (reading < TEMP_VALID_MIN)
	{
		return false;
	}

	return reading >= warningThreshold - ALLOW_NEAR_WARNING_TEMP;
}

char modeStr[5] = { 0, 0, 0, 0, 0 };
const char* GlobalVars::controlMode2char(int feedbackMode, int ctrlMode)
{
	if (feedbackMode == 0)
	{
		modeStr[0] = 'I';
		modeStr[1] = 'd';
		modeStr[2] = 'l';
		modeStr[3] = 'e';
	}
	else if (feedbackMode == 1)
	{
		modeStr[0] = 'O';
		modeStr[1] = 'p';
		modeStr[2] = 'l';
		modeStr[3] = 'p';
	}
	else
	{
		if (ctrlMode == 1)
		{
			modeStr[0] = 'H';
			modeStr[1] = 'e';
			modeStr[2] = 'a';
			modeStr[3] = 't';
		}
		else if (ctrlMode == -1)
		{
			modeStr[0] = 'C';
			modeStr[1] = 'o';
			modeStr[2] = 'o';
			modeStr[3] = 'l';
		}
		else if (ctrlMode == 0)
		{
			modeStr[0] = 'D';
			modeStr[1] = 'u';
			modeStr[2] = 'a';
			modeStr[3] = 'l';
		}
	}
	return modeStr;
}

void MainTempRecord::restart() {
	lastReadingSecond = clock() / 1000;
	fillingPointIdx = 0;
	filledPointCount = 0;
	//Serial.println("Rec restart");
}

bool MainTempRecord::update(float val) {
	int time = clock() / 1000;
	if (time != lastReadingSecond) {
		lastReadingSecond = time;
		temps[fillingPointIdx] = val;

		//Serial.print("Up #");
		//sprintf(GlobalVars::outputBuffer, "%03d", fillingPointIdx);
		//Serial.print(fillingPointIdx);
		//Serial.print(" @");
		//Serial.print(lastReadingSecond);
		//Serial.print(" of ");
		//sprintf(GlobalVars::outputBuffer, "%6.2f", temps[fillingPointIdx]);
		//Serial.println(GlobalVars::outputBuffer);

		fillingPointIdx = (fillingPointIdx + 1) % RECORD_LEN;

		if (filledPointCount < 255) {
			filledPointCount++;
		}
		return true;
	}
	return false;
}

bool MainTempRecord::isSteady(float currentReading) {
	//update(currentReading);
	if (filledPointCount < steadyDurSec) {
		return false;
	}

	int countedSecs = 0;
	float minRead = 999, maxRead = -270;
	for (int idx = fillingPointIdx - 1; idx >= 0; idx--) {
		minRead = min(minRead, temps[idx]);
		maxRead = max(maxRead, temps[idx]);
		countedSecs++;
	}

	for (int idx = RECORD_LEN - 1; (countedSecs < steadyDurSec); idx--) {
		minRead = min(minRead, temps[idx]);
		maxRead = max(maxRead, temps[idx]);
		countedSecs++;
	}

	//Serial.print("IsSteady");
	//Serial.print("Min ");
	//sprintf(GlobalVars::outputBuffer, "%6.2f,", minRead);
	//Serial.print(GlobalVars::outputBuffer);
	//Serial.print("Max ");
	//sprintf(GlobalVars::outputBuffer, "%6.2f,", maxRead);
	//Serial.print(GlobalVars::outputBuffer);

	if (maxRead - minRead <= steadyRange) {
		//Serial.println("Steady");
		return true;
	}

	//Serial.println("Shifting");
	return false;
}

void GlobalVars::heartBeatUpdate() {
	if (MiscFlags::heartBeatInterval > 0) {
		MiscFlags::heartBeatExpire = clock() + MiscFlags::heartBeatInterval;
		if (MiscFlags::heartBeatExpire == 0) {
			MiscFlags::heartBeatExpire = 1;
		}
	}
	else {
		MiscFlags::heartBeatExpire = 0;
	}
}

bool GlobalVars::heartBeatCheck() {
	return (MiscFlags::heartBeatExpire == 0) || (clock() < MiscFlags::heartBeatExpire);
}